/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
 * $Id: ahrs.h,v 1.1 2005/12/15 13:44:22 hecto Exp $
 *
 * Fast AHRS object almost ready for use on microcontrollers
 * Fixed-point mode can be use !
 *
 * (c) 2003 Trammell Hudson <hudson@rotomotion.com>
 * (c) 2005 Jean-Pierre Dumont <jpxDOTdumontATwanadooDOTfr>
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA. 
*/

/** \file ahrs.h
 *  \brief Attitude Heading Reference System (gyros, accels and magneto
 * filtered through a Kalman)
 *
 */

#ifndef __AHRS_
#define __AHRS_

#include <inttypes.h>
//#include "gyro.h"
//#include "accel.h"
//#include "compass.h"
//#include "board.h"

//typedef float		float;
#define			float_min	0.000000001
typedef uint8_t		index_t;

#define IMU_GYROS_CONNECTED_TO_AP 1

#define IMU_FREQUENCY           6

extern uint8_t ahrs_state;

extern float		dcm00;
extern float		dcm01;
extern float		dcm02;
extern float		dcm12;
extern float		dcm22;

/*
 * We have seven variables in our state -- the quaternion attitude
 * estimate and three gyro bias values.  The state time update equation
 * comes from the IMU gyro sensor readings:
 *
 *	Q_dot		= Wxq(pqr) * Q
 *	Bias_dot	= 0
 */

extern float	X[7];
extern float	quat[4];
extern float	q0;
extern float	q1;
extern float	q2;
extern float	q3;
extern float	bias[3];
//extern float	bias_p;
//extern float	bias_q;
//extern float	bias_r;

/*
 * The user inputs the acceleration and rotational rates here, then
 * calls the update functions.
 */

extern float	pqr[3];   // angular rate, rad/s
extern float	accel[3]; // acceleration, g
extern float    mag[3];   // calibrated data from compass

/*
 * The euler estimate will be updated less frequently than the
 * quaternion, but some applications are ok with that.
 */
extern float	ahrs_euler[3];
extern float	ahrs_accel[3];
extern float	ahrs_gyro[3];
extern float	ahrs_euler_zero[3];
extern float	ahrs_euler_real[3];

/** ahrs_state possible values */
#define AHRS_NOT_INITIALIZED		0
#define AHRS_IMU_CALIBRATION		1
#define AHRS_RUNNING			2

/** Current internal state */
extern uint8_t ahrs_state;

///** Restarts the ahrs with zero_calibration phase */
//void ahrs_init( uint8_t do_zero_calibration );

/*
 * Initialize the AHRS state data and covariance matrix.  The user
 * should have filled in the pqr and accel vectors before calling this.
 * They should also have set ahrs_euler[2] from an untilted compass reading
 * and the ahrs_euler[0], ahrs_euler[1] from the accel2roll / accel2pitch values.
 */
extern void ahrs_init();

//Udates state with accels and compas
void ahrs_update( void );

void ahrs_gyro_update( void );

// Update pitch in Kalman filter
extern void ahrs_pitch_update(float pitch);

// Update roll in Kalman filter
extern void ahrs_roll_update(float roll);

// Update yaw in Kalman filter
extern void ahrs_compass_update(float heading);

// Calculate pitch angle
extern void pitch_update(void);

// Calculate roll angle
extern void roll_update(void);

extern void gyro_update(void);

// Calculate calibrated untilted compass data
extern void compass_update(void);

extern inline float accel2roll(void);

extern inline float accel2pitch(void);

extern float untilt_compass(const float *mag);

extern void quat2euler();

/*
 * Call ahrs_state_update every dt seconds with the raw body frame angular
 * rates.  It updates the attitude state estimate.
 */
extern void ahrs_state_update(void);

#endif //__AHRS_
